import cv2
import numpy as np
import math
import time
import imutils
from  imutils.video import VideoStream, videostream
import numpy
import pyzbar.pyzbar as pyzbar
import argparse
import datetime
from PIL import Image,ImageEnhance
import serial
import binascii
import pyrealsense2 as rs



#ser=serial.Serial("/dev/ttyUSB0",115200,timeout=3)
ser=serial.Serial("/dev/ttyUSB0",115200,timeout=3)
recogTime=20#识别时间
gapTime=3#机械臂旋转调整时间
a=1    
code_order=0
us=1#下位机发送的
cnt=0
#颜色识别范围
color_dist={'red':{'Lower':np.array([156,43,60]),'Upper':np.array([100,255,255])},
            'blue':{'Lower':np.array([100,43,46]),'Upper':np.array([124,255,255])},
            'green':{'Lower':np.array([35,43,35]),'Upper':np.array([77,255,255])},}

def recogniseByColor(ball_color):
    #ball_color为要识别的颜色
    # ret,frame=cap.read()
    # frame=imutils.resize(frame,width=300)
    # frame=frame[60:120,120:200]
    gs_frame=cv2.GaussianBlur(color_frame,(5,5),0)#高斯模糊
    hsv=cv2.cvtColor(gs_frame,cv2.COLOR_BGR2HSV)#转化为HSV
    erode_hsv=cv2.erode(hsv,None,iterations=2)#腐蚀，粗的变细
    inRange_hsv=cv2.inRange(erode_hsv,color_dist[ball_color]['Lower'],color_dist[ball_color]['Upper'])
    cnts=cv2.findContours(inRange_hsv.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]
    # cv2.imshow("color",frame)#颜色块展示
    cv2.waitKey(5)
    if(len(cnts)==0):
        time.sleep(0.5)
        print("无该颜色")
        return False
    else:

        if ball_color=='blue':
            return True
            # color="333"
            # print(color)
            # ser.write(color)

        if ball_color=='red':
            return True
            # color="444"
            # print(color)
            # ser.write(color)

        if ball_color=='green':
            return True
            # color="222"
            # print(color)
            # ser.write(color)
        #下面四句是显示效果，可以注释掉
        # c=max(cnts,key=cv2.contourArea)
        # rect=cv2.minAreaRect(c)
        # box=cv2.boxPoints(rect)
        # cv2.drawContours(frame,[np.int0(box)],-1,(0,255,255),2)

        # cv2.imshow('color',frame)
        cv2.waitKey(1)
        return True
    vs.stop()
    cv2.destroyAllWindows()


def barcode_recog_new(color_frame):
    #二维码识别优化版本
    print("[INFO] starting video stream...开始扫描二维码...")
    vs=VideoStream(4).start()
    time.sleep(2.0)
    flag=False
    frame=imutils.resize(frame,width=1000)
    barcodes=pyzbar.decode(frame)
    for barcode in barcodes:
        (x,y,w,h)=barcode.rect
        cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0.255),2)

        barcodeDate=barcode.data.decode("utf-8")

        code_order=barcodeDate
        barcodeType=barcode.type


        barcodeDate=list(barcodeDate)
        #barcodeDate.pop(3)
        barcodeDate=''.join(barcodeDate)
        barcodeDate='a'+barcodeDate+'z'
        print(barcodeDate)
        us=ser.read(10)
        print(us)
        if barcodeDate !="":
            flag=True
    cv2.imshow("Barcode Scanner",color_frame)
    cv2.waitKey(5)
    if flag:
        print("识别成功，解析成功")
    cv2.destroyAllWindows()
   # vs.stop()
    return flag

# Create a pipeline
pipeline = rs.pipeline()

# Create a config and configure the pipeline to stream
#  different resolutions of color and depth streams
config = rs.config()

# Get device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
device = pipeline_profile.get_device()
device_product_line = str(device.get_info(rs.camera_info.product_line))

found_rgb = False
for s in device.sensors:
    if s.get_info(rs.camera_info.name) == 'RGB Camera':
        found_rgb = True
        break
if not found_rgb:
    print("The demo requires Depth camera with Color sensor")
    exit(0)

config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)

if device_product_line == 'L500':
    config.enable_stream(rs.stream.color, 960, 540, rs.format.bgr8, 30)
else:
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# Start streaming
profile = pipeline.start(config)

# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: " , depth_scale)

# We will be removing the background of objects more than
#  clipping_distance_in_meters meters away
clipping_distance_in_meters = 1 #1 meter
clipping_distance = clipping_distance_in_meters / depth_scale

# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
align = rs.align(align_to)


# Streaming loop
try:
    while True:
        # Get frameset of color and depth
        frames = pipeline.wait_for_frames()
        # frames.get_depth_frame() is a 640x360 depth image

        # Align the depth frame to color frame
        aligned_frames = align.process(frames)

        # Get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image
        color_frame = aligned_frames.get_color_frame()

        # Validate that both frames are valid
        if not aligned_depth_frame or not color_frame:
            continue

        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())



        if cnt==0 and us!=1: 
                us=code_order/100
                if cnt==1:
                    us=code_order/10%10
                    if cnt==2:
                        us=code_order%10

                if us==2 or us==3 or us==1:
                    # cap=cv2.VideoCapture(0)
                    # if not cap.isOpened():
                # if pipeline.start(config):
                #         print("摄像头未打开")
                #         exit(0) 

                    message=0

                    for i in "aap":
                        ###绿色：2，蓝色：3，红色：1
                        if us==2:
                            ##################################
                            ball_color='green'
                            time0=time.time()#读取当前时间
                            print("识别绿色")
                            #循环五次
                            while time.time()-time0<recogTime:
                                # ret,frame=cap.read()
                                # if ret:#读取到摄像头
                                    if color_frame is not None:#摄像头有图像
                                        if(recogniseByColor(ball_color)):#识别到了颜色
                                            print("成功识别绿色")
                                            message=message+1
                                            ser.write(message)
                                            break
                                    else:
                                        print("无画面")
                                # else:
                                #     print("无法读取摄像头！")
                            cnt=cnt+1
                    
                            # cap.release()#释放资源
                        # us=us+1#识别下一个颜色
                        # break#颜色识别成功之后，跳出该颜色while的循环-
                        if us==3:
                            
                            ##################################
                            ball_color='blue'
                            time2=time.time()
                            time.sleep(gapTime)
                            print("识别蓝色")
                            while time.time()-time2<recogTime:
                                # ret,frame=cap.read()
                                # if ret:

                                    if color_frame is not None:
                                        if(recogniseByColor(ball_color)):
                                            message=message+1
                                            ser.write(message)
                                            print("成功识别蓝色")
                                            break
                                    else:
                                        print("无画面")
                                # else:
                                #     print("无法读取摄像头！")
                            cnt=cnt+1
                            #us=us+1
                            #break


                        if us==1:
                            ##################################
                            ball_color='red'
                            time1=time.time()
                            time.sleep(gapTime)
                            print("识别红色")
                            while time.time()-time1<recogTime:
                                # ret,frame=cap.read()

                                    if color_frame is not None:
                                        if(recogniseByColor(ball_color)):
                                            message=message+1
                                            ser.write(message)
                                            print("成功识别红色")
                                            break
                                    else:
                                        print("无画面")
                            cnt=cnt+1


        if us==0x01:
            flag=barcode_recog_new(color_frame)
            if flag:
                us=us+1 
            #send("a123321z")
            #time.sleep(1)
            #send("a123321z")





        cv2.namedWindow('Align Example', cv2.WINDOW_NORMAL)
        cv2.imshow('Align Example', color_image)
        key = cv2.waitKey(1)
        # Press esc or 'q' to close the image window
        if key & 0xFF == ord('q') or key == 27:
            cv2.destroyAllWindows()
            break
finally:
    pipeline.stop()




